/**
* Safe Real-time SLAM with webcam/camera input
* Enhanced error handling and memory protection
*/

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include <iomanip>
#include <signal.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/videoio.hpp>
#include "Parameters.h"
#include "System.h"

using namespace std;

// Global variables for cleanup
ORB_SLAM2::System* gSLAM = nullptr;
cv::VideoCapture* gCap = nullptr;
bool gRunning = true;

// Signal handler for graceful shutdown
void signalHandler(int signum) {
    cout << "\nReceived signal " << signum << ". Shutting down gracefully..." << endl;
    gRunning = false;
}

int main(int argc, char **argv)
{
    if(argc < 3 || argc > 4)
    {
        cerr << endl << "Usage: ./mono_realtime_safe path_to_vocabulary path_to_settings [camera_id]" << endl;
        cerr << "Example: ./mono_realtime_safe ./Vocabulary/ORBvoc.txt Examples/config/Realtime.yaml 0" << endl;
        return 1;
    }

    // Set up signal handlers
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);

    // Camera ID (default 0 for webcam)
    int camera_id = 0;
    if(argc == 4)
    {
        camera_id = atoi(argv[3]);
    }

    try {
        // Open camera first and test it
        cv::VideoCapture cap(camera_id);
        gCap = &cap;
        
        if(!cap.isOpened())
        {
            cerr << "Error: Cannot open camera " << camera_id << endl;
            return 1;
        }

        // Set camera properties
        cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
        cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
        cap.set(cv::CAP_PROP_FPS, 30);
        
        // Test camera by reading a frame
        cv::Mat test_frame;
        cap >> test_frame;
        if(test_frame.empty())
        {
            cerr << "Error: Camera opened but cannot read frames" << endl;
            return 1;
        }
        
        cout << "Camera test successful. Frame size: " << test_frame.cols << "x" << test_frame.rows << endl;
        
        // Create SLAM system after camera is confirmed working
        cout << "Creating SLAM system..." << endl;
        ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);
        gSLAM = &SLAM;
        cout << "SLAM system created successfully!" << endl;

        cout << endl << "-------" << endl;
        cout << "Starting real-time SLAM..." << endl;
        cout << "Camera ID: " << camera_id << endl;
        cout << "Press Ctrl+C to quit gracefully" << endl << endl;

        cv::Mat im;
        int frame_count = 0;
        auto start_time = std::chrono::steady_clock::now();
        auto last_status_time = start_time;

        while(gRunning)
        {
            // Capture frame
            cap >> im;
            if(im.empty())
            {
                cerr << "Failed to capture frame at frame " << frame_count << endl;
                usleep(100000); // Wait 100ms
                continue;
            }

            // Get timestamp
            auto current_time = std::chrono::steady_clock::now();
            double timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(current_time - start_time).count();

            try {
                // Process frame with error handling
                cv::Mat pose = SLAM.TrackMonocular(im, timestamp, frame_count, "frame_" + to_string(frame_count));
                
                // Print status every 5 seconds
                if(std::chrono::duration_cast<std::chrono::seconds>(current_time - last_status_time).count() >= 5)
                {
                    cout << "Frame: " << frame_count 
                         << ", Time: " << fixed << setprecision(2) << timestamp 
                         << "s, FPS: " << frame_count / timestamp;
                    
                    if(!pose.empty())
                    {
                        cout << ", Tracking: OK";
                    }
                    else
                    {
                        cout << ", Tracking: LOST";
                    }
                    cout << endl;
                    
                    last_status_time = current_time;
                }
            }
            catch(const std::exception& e)
            {
                cerr << "SLAM processing error: " << e.what() << endl;
                usleep(100000); // Wait 100ms before continuing
            }

            frame_count++;
            
            // Small delay to prevent excessive CPU usage
            usleep(1000); // 1ms delay
        }

        cout << "\nShutting down SLAM system..." << endl;
        
        // Cleanup
        cap.release();
        gCap = nullptr;
        
        // Stop all threads
        SLAM.Shutdown();
        gSLAM = nullptr;

        // Save camera trajectory
        SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_realtime_safe.txt");

        cout << "Safe real-time SLAM finished!" << endl;
        cout << "Total frames processed: " << frame_count << endl;
        auto final_time = std::chrono::steady_clock::now();
        double total_time = std::chrono::duration_cast<std::chrono::duration<double>>(final_time - start_time).count();
        if(total_time > 0)
            cout << "Average FPS: " << frame_count / total_time << endl;
        
        return 0;
    }
    catch(const std::exception& e)
    {
        cerr << "Fatal error: " << e.what() << endl;
        
        // Cleanup on error
        if(gCap)
        {
            gCap->release();
        }
        if(gSLAM)
        {
            gSLAM->Shutdown();
        }
        
        return 1;
    }
}
